-- Copyright The Slam Authors

POSE_GRAPH = {
  optimize_every_n_nodes = 90,
  constraint_builder = {
    sampling_ratio = 0.3,
    max_constraint_distance = 15.,
    min_score = 0.55,
    global_localization_min_score = 0.6,
    loop_closure_translation_weight = 1.1e4,
    loop_closure_rotation_weight = 1e5,
    log_matches = true,
    localization_mode = false,
    mapping_mode = false,
    match_scores_queue_length = 3,
    fast_correlative_scan_matcher = {
      linear_search_window = 7.,
      angular_search_window = math.rad(30.),
      branch_and_bound_depth = 7,
      use_dynamic_parameters_after_n_nodes=1,
      min_score=0.4,
      max_score=0.5,
      score_steps=1,
      min_linear_search_window=0.5,
      max_linear_search_window=2,
      linear_search_window_steps=3,
      min_angular_search_window=math.rad(20.),
      max_angular_search_window=math.rad(30.),
      angular_search_window_steps=1,
    },
    ceres_scan_matcher = {
      occupied_space_weight = 100.0, --20.
      translation_weight = 1.0, --10.
      rotation_weight = 1.0,
      ceres_solver_options = {
        use_nonmonotonic_steps = true,
        max_num_iterations = 10,
        num_threads = 4, --1
      },
    },
    fast_correlative_scan_matcher_3d = {
      branch_and_bound_depth = 8,
      full_resolution_depth = 3,
      min_rotational_score = 0.77,
      min_low_resolution_score = 0.55,
      linear_xy_search_window = 5.,
      linear_z_search_window = 1.,
      angular_search_window = math.rad(15.),
    },
    ceres_scan_matcher_3d = {
      occupied_space_weight_0 = 5.,
      occupied_space_weight_1 = 30.,
      translation_weight = 10.,
      rotation_weight = 1.,
      only_optimize_yaw = false,
      ceres_solver_options = {
        use_nonmonotonic_steps = false,
        max_num_iterations = 10,
        num_threads = 1,
      },
    },
  },
  matcher_translation_weight = 5e2,
  matcher_rotation_weight = 1.6e3,
  optimization_problem = {
    huber_scale = 1e1,
    acceleration_weight = 1e3,
    rotation_weight = 3e5,
    local_slam_pose_translation_weight = 1e5,
    local_slam_pose_rotation_weight = 1e5,
    odometry_translation_weight = 1e3,
    odometry_rotation_weight = 1e1,
    fixed_frame_pose_translation_weight = 1e1,
    fixed_frame_pose_rotation_weight = 1e2,
    fixed_frame_pose_use_tolerant_loss = false,
    fixed_frame_pose_tolerant_loss_param_a = 1,
    fixed_frame_pose_tolerant_loss_param_b = 1,
    log_solver_summary = false,
    use_online_imu_extrinsics_in_3d = true,
    fix_z_in_3d = false,
    ceres_solver_options = {
      use_nonmonotonic_steps = false,
      max_num_iterations = 50,
      num_threads = 7,
    },
  },
  slam_evaluator = {
    distance_traveled_threshold = 10,
    angle_traveled_threshold = math.rad(180.),
    distance_traveled_threshold_initial = 2,
    angle_traveled_threshold_initial = math.rad(90.),
  },
  max_num_final_iterations = 200,
  global_sampling_ratio = 0.003,
  log_residual_histograms = true,
  global_constraint_search_after_n_seconds = 10.,

  use_constraint_check = false,
  submap_interval_check_constraint = 4,
  max_delta_translation_check_constraint = 1,
  max_delta_rotation_check_constraint = math.rad(5.),

  use_reflector = false,
  reflector_fusion_min_num = 2,
  reflector_fusion_pose_error_t = 0.1,
  reflector_fusion_pose_error_r = 0.06,
  reflector_dector = { 
    match_distance_threshold = 0.2,
    reflector_translation_weight = 1e6,
    reflector_rotation_weight = 0,
    fusion_translation_weight = 1e6,
    fusion_rotation_weight = 1e6,
    new_reflector_min_distance_threshold = 0.5,
  },
}
